#include "direct.h"
#define mat_elem(a, y, x, n) (a + ((y) * (n) + (x)))

void swap_row(double *a, double *b, int r1, int r2, int n)
{
	double tmp, *p1, *p2;
	int i;
 
	if (r1 == r2) return;
	for (i = 0; i < n; i++) {
		p1 = mat_elem(a, r1, i, n);
		p2 = mat_elem(a, r2, i, n);
		tmp = *p1, *p1 = *p2, *p2 = tmp;
	}
	tmp = b[r1], b[r1] = b[r2], b[r2] = tmp;
}
 
void ca_gauss_dense(double *A, double *B, double *x, int n)
{
#define A(y, x) (*mat_elem(a, y, x, n))
	int i, j, col;
	double max, tmp;
	double a[n*n], b[n];

	for(i = 0; i < n; ++i){
		for(j = 0; j < n; ++j)
			a[i*n+j] = A[i*n+j];			
		b[i] = B[i];
	}

 
	for (col = 0; col < n; col++) {
		j = col, max = A(j, j);
 
		for (i = col + 1; i < n; i++)
			if ((tmp = fabs(A(i, col))) > max)
				j = i, max = tmp;
 
		swap_row(a, b, col, j, n);
 
		for (i = col + 1; i < n; i++) {
			tmp = A(i, col) / A(col, col);
			for (j = col + 1; j < n; j++)
				A(i, j) -= tmp * A(col, j);
			A(i, col) = 0;
			b[i] -= tmp * b[col];
		}
	}
	for (col = n - 1; col >= 0; col--) {
		tmp = b[col];
		for (j = n - 1; j > col; j--)
			tmp -= x[j] * A(col, j);
		x[col] = tmp / A(col, col);
	}
#undef A
}

void ca_chol_dense(double *MNA, double *b, int n, double *x)
{
	int i,j,k;
	double chol[n][n];
	
	double sum;
	double y[n];
	
	for(i=0;i<n;i++){
	  chol[i][i] = MNA[i*n+i];
	  for(k=0;k<i;k++){
	    chol[i][i] -= chol[k][i]*chol[k][i];
	  }
	  chol[i][i] = sqrtf(chol[i][i]);
	  
	  
	  for(j=i+1;j<n;j++){
	    chol[i][j] = MNA[i*n+j];
	    for(k=0;k<i;k++)
	      chol[i][j] -= chol[k][i]*chol[k][j];
	    
	    chol[i][j] /= chol[i][i];
	  }
	  
	}
	/* Solution */

	/* Forward substitution */
	for(k = 0; k < n; ++k){
		for ( j = 0; j < k; ++j)
			b[k] -= chol[j][k]*y[j];
		y[k] = b[k]/chol[k][k];  
	}
        
	/* Backward substitution */
	for(k = n-1; k >= 0; --k){
		for(j = k+1; j < n; j++)
			y[k] -= (chol[k][j]*x[j]);
		x[k] = y[k]/chol[k][k];  
	}
}

void ca_gauss_sparse(cs *MNA, double *b, int n, double *x)
{
	css *S;	csn *N;
	double y[n];
	
	//Factorization
	S = cs_sqr(2, MNA, 0);	
	N = cs_lu(MNA, S, 1);

	//Solution
	cs_ipvec(N->pinv, b, y, n);
	//solve L
	cs_lsolve(N->L, y);	//in/out x
	//solve U
	cs_usolve(N->U, y);
	//full permutation
	cs_ipvec(S->q, y, x, n);

	cs_sfree(S);
	cs_nfree(N);
}

void ca_chol_sparse(cs *MNA, double *b, int n, double *x)
{
	css *S;	csn *N;
	double y[n];

	//Factorization
	S = cs_schol(1, MNA);
	N = cs_chol(MNA, S);
			
	cs_ipvec(S->pinv, b, y, n);
	//solve L		
	cs_lsolve(N->L, y);	//in/out x
	//solve U
	cs_ltsolve(N->L, y);
	//full permutation
	cs_pvec(S->pinv, y, x, n);
}
